//
// File: jac_obt_vR.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 02-Dec-2024 17:27:20
//

// Include Files
#include "jac_obt_vR.h"
#include "norm.h"
#include <cstring>

// Variable Definitions
static double dx_a[32];

static double dy_a[32];

static double dsig_a[32];

// Function Definitions
//
// aIN,Fnk_pre,S_pre,Outputs_pre,aph,b,xd,yd,x,y,xo,yo,R2,R5,R9
// tar=[xd,yd]
// car_xy=[x,y]
// obs_p=[xo,yo]
// pre_mat=[R2,R5,R9]
//
// Arguments    : const double a[32]
//                const double Fnk_pre[16]
//                double S_pre
//                const double Outputs_pre[2]
//                double aph
//                double b
//                const double tar[2]
//                const double car_xy[2]
//                const double obs_p[2]
//                const double pre_mat[3]
//                double dg_a[32]
//                double do_a[32]
//                double sig_a[32]
//                double sio_a[32]
// Return Type  : void
//
void jac_obt_vR(const double[32], const double Fnk_pre[16], double S_pre,
                const double Outputs_pre[2], double aph, double b,
                const double tar[2], const double car_xy[2],
                const double obs_p[2], const double pre_mat[3], double dg_a[32],
                double do_a[32], double sig_a[32], double sio_a[32])
{
  double Fnk2[32];
  double a_tmp;
  double b_a_tmp;
  double c_a_tmp;
  double d;
  double d1;
  double d2;
  double d3;
  double d4;
  double d_a_tmp;
  double sig_a_tmp;
  double sio_a_tmp;
  // 权重数量，一般1*32
  for (int i{0}; i < 16; i++) {
    d = Fnk_pre[i];
    Fnk2[i] = d;
    Fnk2[i + 16] = d;
  }
  //  初始化静态变量
  //
  a_tmp = car_xy[0] - tar[0];
  b_a_tmp = car_xy[1] - tar[1];
  c_a_tmp = car_xy[0] - obs_p[0];
  d_a_tmp = car_xy[1] - obs_p[1];
  d = Outputs_pre[0];
  d1 = Outputs_pre[1];
  d2 = pre_mat[0];
  d3 = pre_mat[1];
  d4 = pre_mat[2];
  sig_a_tmp = a_tmp * a_tmp + b_a_tmp * b_a_tmp;
  sio_a_tmp = c_a_tmp * c_a_tmp + d_a_tmp * d_a_tmp;
  for (int i{0}; i < 32; i++) {
    double Outputs_i;
    double b_a;
    double deV;
    double deV_tmp;
    if (i + 1 <= 16) {
      Outputs_i = d;
    } else {
      Outputs_i = d1;
    }
    b_a = aph * Outputs_i + 2.0;
    deV_tmp = Fnk2[i] * S_pre;
    deV = deV_tmp / (2.0 * (b_a * b_a));
    // v对于a的偏导数
    if (i + 1 <= 16) {
      b_a = aph * Outputs_i + 2.0;
      Outputs_i = deV_tmp / (2.0 * b * (b_a * b_a));
      // w对于a的偏导数
    } else {
      b_a = aph * Outputs_i + 2.0;
      Outputs_i = -deV_tmp / (2.0 * b * (b_a * b_a));
      // w对于a的偏导数
    }
    dx_a[i] += d2 * deV;
    dy_a[i] += d3 * deV;
    dsig_a[i] += d4 * Outputs_i;
    //
    dg_a[i] =
        (a_tmp * dx_a[i] + b_a_tmp * dy_a[i]) / coder::b_norm(a_tmp, b_a_tmp);
    do_a[i] = (c_a_tmp * dx_a[i] + d_a_tmp * dy_a[i]) /
              coder::b_norm(c_a_tmp, d_a_tmp);
    Outputs_i = dx_a[i];
    b_a = dy_a[i];
    deV_tmp = dsig_a[i];
    sig_a[i] =
        (a_tmp / sig_a_tmp * b_a - b_a_tmp / sig_a_tmp * Outputs_i) - deV_tmp;
    sio_a[i] =
        (c_a_tmp / sio_a_tmp * b_a - d_a_tmp / sio_a_tmp * Outputs_i) - deV_tmp;
    //
  }
}

//
// aIN,Fnk_pre,S_pre,Outputs_pre,aph,b,xd,yd,x,y,xo,yo,R2,R5,R9
// tar=[xd,yd]
// car_xy=[x,y]
// obs_p=[xo,yo]
// pre_mat=[R2,R5,R9]
//
// Arguments    : void
// Return Type  : void
//
void jac_obt_vR_init()
{
  std::memset(&dx_a[0], 0, 32U * sizeof(double));
  std::memset(&dy_a[0], 0, 32U * sizeof(double));
  std::memset(&dsig_a[0], 0, 32U * sizeof(double));
}

//
// File trailer for jac_obt_vR.cpp
//
// [EOF]
//
